#include "kalmanfilter.h"

// 卡尔曼滤波器初始化
void Kalman_Init(KalmanFilter_t *kf, float initial_state, float initial_covariance, float process_noise, float measurement_noise)
{
    kf->x = initial_state;      // 初始SOC估计
    kf->P = initial_covariance; // 初始协方差
    kf->Q = process_noise;      // 过程噪声协方差
    kf->R = measurement_noise;  // 测量噪声协方差
}

// 更新步骤
void Kalman_Update(KalmanFilter_t *kf, float measurement)
{
    float K = kf->P / (kf->P + kf->R);           // 卡尔曼增益
    kf->x   = kf->x + K * (measurement - kf->x); // 更新状态估计
    kf->P   = (1 - K) * kf->P;                   // 更新协方差
    kf->P += kf->Q;                              // 加入过程噪声
}
